#!/usr/bin/env python
# encoding: utf-8
# from Controller import *
import threading
import SocketServer
import time

flag = False
is_ok = False
step = 5
pitch_angle = 90
yaw_angle = 90

IP_ADDRESS = "192.168.1.109"
PORT = 1234


# socket通信
class MyServer(SocketServer.BaseRequestHandler):
    def handle(self):
        conn = self.request
        conn.sendall(bytes("YunTai>>>Hello, I'm YunTai"))
        print "一个设备已连接"
        # mController = Controller()
        global pitch_angle, yaw_angle,step
        while flag:
            ret_bytes = conn.recv(64)
            ret_str = str(ret_bytes)
            # print ret_str
            if ret_str == "f":  # 收到向前的命令
                pitch_angle -= step
            elif ret_str == "b":  # 向后
                pitch_angle += step
            elif ret_str == "l":  # 向左
                yaw_angle -= step
            elif ret_str == "r":  # 向右
                yaw_angle += step

            if yaw_angle > 165:
                yaw_angle = 165
            elif yaw_angle < 15:
                yaw_angle = 15
            if pitch_angle > 120:
                pitch_angle = 120
            elif pitch_angle < 1:
                pitch_angle = 1
            print pitch_angle,yaw_angle
            # mController.control_pitch(pitch_angle)
            # mController.control_yaw(yaw_angle)
        conn.sendall(bytes("YunTai>>>Goodbye..."))


def my_socket_server():
    global is_ok
    while not is_ok:
        try:
            server = SocketServer.ThreadingTCPServer((IP_ADDRESS, PORT), MyServer)
            server.serve_forever()
            is_ok = True
        except:
            time.sleep(1)
            print "Not ready yet!"


if __name__ == '__main__':
    try:
        thread1 = threading.Thread(target=my_socket_server)
        thread1.setDaemon(True)
        thread1.start()
        flag = True
        while flag:
            time.sleep(1)
    except (KeyboardInterrupt, SystemExit):
        flag = False
        time.sleep(2)
